//
// Created by bobin on 18-1-16.
//

#include <iostream>
#include <string>
#include <opencv2/opencv.hpp>
#include <opencv/highgui.h>
#include "utility.h"
#include "sgm_stereo.h"
#include "test_utility.h"
#include<iomanip>
#include <vector>
#include <dirent.h>
#include "open_chisel/Chisel.h"
#include <open_chisel/ProjectionIntegrator.h>
#include <open_chisel/camera/PinholeCamera.h>
#include <open_chisel/camera/DepthImage.h>
#include <open_chisel/camera/ColorImage.h>
#include <open_chisel/pointcloud/PointCloud.h>
#include <open_chisel/truncation/QuadraticTruncator.h>
#include <open_chisel/weighting/ConstantWeighter.h>

#include <open_chisel/truncation/QuadraticTruncator.h>
#include <open_chisel/truncation/InverseTruncator.h>
#include "KittiReader.h"

using namespace std;
typedef float DepthData;
typedef uint8_t ColorData;

bool SaveMesh(chisel::ChiselPtr chiselMap, string file_name) {
    bool saveSuccess = chiselMap->SaveAllMeshesToPLY(file_name);
    return saveSuccess;
}

void IntegrateLastPointCloud(chisel::ChiselPtr chiselMap, chisel::ProjectionIntegrator projectionIntegrator,
                             const chisel::PointCloud& lastPointCloud,
                             float farPlaneDist, chisel::Transform lastPose) {
    cout << "Integrating point cloud";
    chiselMap->IntegratePointCloud(projectionIntegrator, lastPointCloud, lastPose, 0.1f, farPlaneDist);

    chiselMap->UpdateMeshes();


}

void IntegrateLastDepthImage(chisel::ChiselPtr chiselMap, chisel::ProjectionIntegrator projectionIntegrator,
                             std::shared_ptr<chisel::DepthImage<DepthData>> lastDepthImage,
                             chisel::PinholeCamera cameraModel, chisel::Transform lastPose) {

    cout << "CHISEL: Integrating depth scan";
    auto start = std::chrono::system_clock::now();
    chiselMap->IntegrateDepthScan<DepthData>(projectionIntegrator, lastDepthImage, lastPose,
                                             cameraModel);

    std::chrono::duration<double> elapsed = std::chrono::system_clock::now() - start;
    cout << "CHISEL: Done with scan" << elapsed.count() * 1000 << "ms";
//        PublishLatestChunkBoxes();
//        PublishDepthFrustum();

    start = std::chrono::system_clock::now();
    cout << "CHISEL: Updating meshes";
    chiselMap->UpdateMeshes();

    elapsed = std::chrono::system_clock::now() - start;
    cout << "CHISEL: Done with mesh" << elapsed.count() * 1000 << " ms";


}


float getInterpolate(const cv::Mat mat, const float x, const float y) {
    int ix = (int) x;
    int iy = (int) y;

    float tl = (float) static_cast<u_char>(mat.at<cv::Vec3b>(iy, ix)[0]);
    float tr = (float) static_cast<u_char>(mat.at<cv::Vec3b>(iy, ix + 1)[0]);
    float bl = (float) static_cast<u_char>(mat.at<cv::Vec3b>(iy + 1, ix)[0]);
    float br = (float) static_cast<u_char>(mat.at<cv::Vec3b>(iy + 1, ix + 1)[0]);

    float dx = x - ix;
    float dy = y - iy;

    float topInt = dx * tr + (1 - dx) * tl;
    float botInt = dx * br + (1 - dx) * bl;
    float leftInt = dy * bl + (1 - dy) * tl;
    float rightInt = dy * br + (1 - dy) * tr;

    float value = dx * rightInt + (1 - dx) * leftInt;
    return value;

}

int main(int argc, char **argv) {

    int s = 4;
    std::stringstream sequence;
    sequence << std::setfill('0') << std::setw(2) << s;
    string data_root = "/home/bobin/data/kitti/sequences/";
    std::string left_img_path_orig = data_root + sequence.str() + "/image_0";
    std::string right_img_path_orig = data_root + sequence.str() + "/image_1";
    std::string output_colored_disparitymap_orig =
            "/home/bobin/data/kitti/sequences/" + sequence.str() + "/disparity";
    string configFile = data_root + sequence.str() + "/KITTI04-12.yaml";
    string groundTruthFolder = data_root + sequence.str() + "/04.txt";
    system(("mkdir " + output_colored_disparitymap_orig).c_str());

    chisel::ChiselPtr chiselMap;
    int chunkSizeX, chunkSizeY, chunkSizeZ;
    chunkSizeX = chunkSizeY = chunkSizeZ = 32;
    float voxelResolution = 0.03;
    bool useColor = true;
    float truncationDistScale = 8.0;
    chiselMap.reset(new chisel::Chisel(Eigen::Vector3i(chunkSizeX, chunkSizeY, chunkSizeZ), voxelResolution, useColor));
    chisel::ProjectionIntegrator projectionIntegrator;
    chisel::TruncatorPtr truncator(new chisel::InverseTruncator(truncationDistScale));

    float carvingDist = 0.05;
    float weight = 1;
    projectionIntegrator.SetCentroids(chiselMap->GetChunkManager().GetCentroids());
    projectionIntegrator.SetTruncator(truncator);
    projectionIntegrator.SetWeighter(chisel::WeighterPtr(new chisel::ConstantWeighter(weight)));
    projectionIntegrator.SetCarvingDist(carvingDist);
    projectionIntegrator.SetCarvingEnabled(true);


    chisel::PinholeCamera pinholeCamera;
    cv::FileStorage fsSettings(configFile, cv::FileStorage::READ);
    if (!fsSettings.isOpened()) {
        cerr << "ERROR: Wrong path to settings" << endl;
        return -1;
    }

    float fx = fsSettings["Camera.fx"];
    float fy = fsSettings["Camera.fy"];
    float cx = fsSettings["Camera.cx"];
    float cy = fsSettings["Camera.cy"];
    float bf = fsSettings["Camera.bf"];
    int width = fsSettings["Camera.width"];
    int height = fsSettings["Camera.height"];
    chisel::Intrinsics intrinsics;
    intrinsics.SetFx(fx);
    intrinsics.SetFy(fy);
    intrinsics.SetCx(cx);
    intrinsics.SetCy(cy);
    pinholeCamera.SetIntrinsics(intrinsics);
    pinholeCamera.SetWidth(width);
    pinholeCamera.SetHeight(height);

    TrajectoryType gtTraj;   //ground truth trajectory


    LoadGroundTruthTraj(groundTruthFolder, gtTraj);
    struct dirent *ptr;
    DIR *dir;
    string PATH = left_img_path_orig;
    dir = opendir(PATH.c_str());
    int counter = 0;

    while ((ptr = readdir(dir)) != NULL) {
        if (ptr->d_name[0] == '.')
            continue;
        counter++;
    }

    for (int number = 0; number < counter; number++) {
        std::stringstream ss;
        ss << std::setfill('0') << std::setw(6) << number;

        std::string left_img_path = left_img_path_orig + "/" + ss.str() + ".png";
        std::string right_img_path = right_img_path_orig + "/" + ss.str() + ".png";
        std::string output_colored_disparitymap = output_colored_disparitymap_orig + "/" + ss.str() + ".png";
        int levels_disparity = 200;

        unsigned char *img_left = NULL, *img_right = NULL;
        size_t width, height;
        loadImageRGB(left_img_path.c_str(), width, height,
                     img_left);/*loading the gray image as RGB is fine, [gray, gray, gray]*/
        loadImageRGB(right_img_path.c_str(), width, height, img_right);
        int size = width * height, size3 = width * height * 3;
        unsigned char *img_tmp = new unsigned char[size3];
        float *guidance_left = new float[size3];

        std::cout << "Begin to calculate the disparity map..." << std::endl;
        deeprob::cv::SGMStereo matcher((int) width, (int) height, levels_disparity, 3, 3);
        deeprob::cv::BoxFilter(img_left, img_tmp, 1, 1, width, height, 3);/*pre-smoothing on guidance_left*/
        for (int i = 0; i < size3; ++i) guidance_left[i] = img_tmp[i];

        matcher.setImage(img_left, img_right, 3);
        matcher.setGuidance(guidance_left, NULL, 3);
        matcher.calDisparity(); /*calculate the disparity map*/
        chisel::Transform pose(gtTraj[number].cast<float>());
#if USE_Depth
        cv::Mat disparity(height, width, CV_8UC3);
        chisel::Transform pose(gtTraj[number].cast<float>());
        shared_ptr<chisel::DepthImage<DepthData>> depthImage = make_shared<chisel::DepthImage<DepthData>>(width,
                                                                                                          height);

        DepthData *depthImageData = depthImage->GetMutableData();
        for (int i = 0; i < size3; i++) {
            depthImageData[i] = bf / matcher._disparity[0][i];
        }
        IntegrateLastDepthImage(chiselMap, projectionIntegrator, depthImage, pinholeCamera, pose);
#else
        chisel::PointCloud cloud;

        for (int j = 0; j < height; ++j) {
            for (int i = 0; i < width; ++i) {
                chisel::Vec3 pt;
                float d = bf / matcher._disparity[0][j * height + i];
                pt[0] = d * (i - cx) / fx;
                pt[1] = d * (j - cy) / fy;
                pt[2] = d ;
                cloud.AddPoint(pt);
            }
        }
        float farPlaneDist = 50.0;
        IntegrateLastPointCloud(chiselMap, projectionIntegrator, cloud, farPlaneDist, pose);
#endif

        std::cout << "Done! Sequence " << s << " number " << number << " is saved." << std::endl;
//        showDataFalseColor(matcher._disparity[0], width, height, disparity.data,
//                           output_colored_disparitymap.c_str(), true);/*save the disparity map in false color*/
//        cv::imshow("disparity", disparity);

//        cv::waitKey(10);
        delete[]img_tmp;
        delete[]guidance_left;
        delete[]img_left;
        delete[]img_right;

    }

    SaveMesh(chiselMap, "map.ply");
    return 1;
}
